
FIQ_Stack	EQU		     0x40001000        ; 512 bytes         	
IRQ_Stack   EQU          0x40000E00        ; 3584 bytes         
SVC_Stack 	EQU          0x00200000        ; 256K bytes 
USR_Stack  	EQU      	 0x00600000        ; 512K bytes  
            
											  
;	IMPORT  FIQ_Handler
;	EXPORT  FIQ_Handler
;	IMPORT  IRQ_Handler
;	EXPORT  IRQ_Handler
;	IMPORT  SWI_Handler
;	EXPORT  SWI_Handler
;	IMPORT  Undefined_Handler
;	EXPORT  Undefined_Handler
;	IMPORT  Abort_Handler
;	EXPORT  Abort_Handler
;	IMPORT  Get_Addr
;	EXPORT  Get_Addr 
	
	IMPORT  SWIHandler
	IMPORT  UndefinedHandler
	IMPORT  AbortHandler
	IMPORT  s_SetFIQHandler
    IMPORT  Main
;	EXPORT  MMU_Init 
;	IMPORT  MMU_Init
    
	PRESERVE8	;dulinfeng add 2007.2.8
	AREA Init1, CODE, READONLY
	
    EXTERN 	FIQ_HANDLER
    EXTERN 	IRQ_HANDLER_TABLE
	
;   EXPORT  __main
;__main 
;
;__entry 
ENTRY
        B       Reset_Handler
        B       Undefined_Handler
        B       SWI_Handler
        B       Abort_Handler
        B       Abort_Handler
        b .		    ;handlerReserved;NOP
        B       IRQ_Handler
        B       FIQ_Handler
		
Undefined_Handler
        B       UndefinedHandler
                
Abort_Handler
        B       AbortHandler

JUMP_Handler
		BX      r3
		
FIQ_Handler
		SUB     lr, lr, #4		
		STMFD   sp!, {r0-r3, lr} 
		LDR     r1, =FIQ_HANDLER
		LDR     r3, [r1,#0x00]
		BL      JUMP_Handler
        LDMFD   sp!, {r0-r3, pc}^
                   
SWI_Handler
        STMFD   sp!, {r0-r3,lr} 
        MOV     R0, #0x13
        MSR     CPSR_c, R0
        BL      SWIHandler                      
        LDMFD   sp!, {r0-r3,pc}^
           
IRQ_Handler
        SUB     lr, lr, #4	
		STMFD   sp!, {r0-r3, r12, lr} 	
        MOV     r0, #0x4A000000     
        LDR     r1, [r0, #0x14] 
		LDR     r2, =IRQ_HANDLER_TABLE
        LDR     r3, [r2, r1, lsl #2] 
        BL      JUMP_Handler      				
        LDMFD   sp!, {r0-r3, r12, pc}^       			


;.EQU R1_C,       (1<<2)
;.EQU R1_I,       (1<<12)
;.EQU R1_M,       (1)
;.EQU R1_A,       (1<<1)
;==============
R1_M    EQU      (1)
R1_I    EQU      (1<<12)
R1_C    EQU      (1<<2)
R1_A    EQU		 (1<<1)

;==================

;EXPORT MMU_EnableDCache
;IMPORT MMU_EnableDCache 
MMU_EnableDCache        
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_C
   mcr  p15,0,r0,c1,c0,0
   MOV  pc,lr

;EXPORT MMU_DisableDCache
;IMPORT MMU_DisableDCache 
MMU_DisableDCache      
   mrc  p15,0,r0,c1,c0,0
   bic  r0,r0,#R1_C
   mcr  p15,0,r0,c1,c0,0
   MOV  pc,lr

;EXPORT  MMU_CleanInvalidateDCacheIndex
;IMPORT  MMU_CleanInvalidateDCacheIndex
MMU_CleanInvalidateDCacheIndex  
   mcr  p15,0,r0,c7,c14,2
   MOV  pc, lr

;EXPORT MMU_InvalidateDCache 
;IMPORT MMU_InvalidateDCache 
MMU_InvalidateDCache
   mcr  p15,0,r0,c7,c6,0
   MOV  pc, lr

;EXPORT MMU_EnableICache 
;IMPORT MMU_EnableICache 
MMU_EnableICache        
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_I
   mcr  p15,0,r0,c1,c0,0
   MOV  pc, lr

;EXPORT  MMU_DisableICache
;IMPORT  MMU_DisableICache
MMU_DisableICache       
   mrc  p15,0,r0,c1,c0,0
   bic  r0,r0,#R1_I
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr

;EXPORT  MMU_InvalidateICache
;IMPORT  MMU_InvalidateICache
MMU_InvalidateICache
   mcr  p15,0,r0,c7,c5,0
   mov  pc,lr

;EXPORT  MMU_SetDomain
;IMPORT  MMU_SetDomain
MMU_SetDomain
   mcr  p15,0,r0,c3,c0,0
   MOV pc, lr

;EXPORT  MMU_SetTTBase
;IMPORT  MMU_SetTTBase
MMU_SetTTBase
   mcr  p15,0,r0,c2,c0,0
   mov  pc,lr

;EXPORT  MMU_SetAsyncBusMode
;IMPORT  MMU_SetAsyncBusMode
MMU_SetAsyncBusMode
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#0xC0000000
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr


;EXPORT  MMU_EnableMMU
;IMPORT  MMU_EnableMMU
MMU_EnableMMU
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_M
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr

;EXPORT  MMU_DisableMMU
;IMPORT  MMU_DisableMMU
MMU_DisableMMU
   mrc  p15,0,r0,c1,c0,0
   bic  r0,r0,#R1_M
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr
   
;EXPORT  MMU_EnableAlignFault 
;IMPORT  MMU_EnableAlignFault
MMU_EnableAlignFault
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_A
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr

;EXPORT  MMU_InvalidateTLB
;IMPORT  MMU_InvalidateTLB
MMU_InvalidateTLB      
   mcr  p15,0,r0,c8,c7,0
   MOV  pc,lr
   
;EXPORT  MMU_SetProcessId
;IMPORT  MMU_SetProcessId   
MMU_SetProcessId        
   mcr  p15,0,r0,c13,c0,0
   MOV  pc,lr


MMU_Init
        STMFD    sp!,{r4-r6,lr}
        BL       MMU_DisableDCache
        BL       MMU_DisableICache
        MOV      r5,#0
Ladd1
        MOV      r4,#0
        MOV      r6,r5,LSL #26
Ladd2
        ORR      r0,r6,r4,LSL #5
        BL       MMU_CleanInvalidateDCacheIndex
        ADD      r4,r4,#1
        CMP      r4,#8
        BCC      Ladd2
        ADD      r5,r5,#1
        CMP      r5,#0x40
        BCC      Ladd1
        BL       MMU_InvalidateICache
        BL       MMU_DisableMMU
        BL       MMU_InvalidateTLB
        LDR      r0,Ladd12
        MOV      r1,#0
        MOV      r5,#0
        MOV      r2,r0
Ladd3
        ADD      r1,r1,#1
        CMP      r1,#0x1000
        STR      r5,[r2],#4
        BCC      Ladd3
        LDR      lr,Ladd16
        MOV      r2,r0
        MOV      r1,#0
        ADD      r12,lr,#0x800
        B        Ladd5
Ladd4
        CMP      r1,#2
        BCC      Ladd5
        CMP      r1,#5
        ADDLS    r3,r12,r1,LSL #20
        BLS      Ladd6
Ladd5
        ADD      r3,lr,r1,LSL #20
Ladd6
        ADD      r1,r1,#1
        CMP      r1,#8
        STR      r3,[r2],#4
        BCC      Ladd4
        LDR      r1,Ladd20
        LDR      r2,Ladd24
        LDR      r12,Ladd36
        STR      r2,[r1],#0x200
        ADD      r2,r2,#0x8000000
        STR      r2,[r1,#0]
        LDR      r2,Ladd32
        LDR      r1,Ladd28
        STR      r2,[r1,#0]
        ADD      r2,r1,#0x600
        MOV      r1,#0
Ladd7
        ADD      r3,r12,r1,LSL #20
        ADD      r1,r1,#1
        CMP      r1,#4
        STR      r3,[r2],#4
        BCC      Ladd7
        LDR      r1,Ladd40
        LDR      r2,Ladd44
        LDR      r3,Ladd48
        ADD      r4,r1,#0x200
        STR      r2,[r1,#0]
        MOV      r1,#0
Ladd8
        ADD      r2,r3,r1,LSL #20
        ADD      r1,r1,#1
        CMP      r1,#0x180
        STR      r2,[r4],#4
        BCC      Ladd8
        BL       MMU_SetTTBase
        LDR      r0,Ladd52
        BL       MMU_SetDomain
        MOV      r0,#0
        BL       MMU_SetProcessId
        BL       MMU_EnableAlignFault
        BL       MMU_SetAsyncBusMode
        BL       MMU_EnableMMU
        BL       MMU_EnableICache
        BL       MMU_EnableDCache
        LDMFD    sp!,{r4-r6,pc}

Ladd12       
	DCD       0x30600000
Ladd16
	DCD       0x3000040E
Ladd20 
	DCD     	0x30600200
Ladd24 
	DCD     	0x0800040E
Ladd28 
	DCD    	0x30600600
Ladd32 
	DCD     	0x18000402
Ladd36 
	DCD    	0x00000402
Ladd40 
	DCD    	0x30601000
Ladd44 
	DCD    	0x40000c02
Ladd48 
	DCD     	0x48000402
Ladd52 
	DCD     	0x55555555

        
Get_Addr 
		MOV     pc, lr
 
Reset_Handler 
		MOV     R0, #0xdf
        MSR     CPSR_c, R0
        LDR     R13, =FIQ_Stack             
        BL      Get_Addr                        
        CMP     r14,#0x40000000
        BLS     Next_CMP
        BL      MMU_Init
        MOV     r0, #0x00
        MOV     pc, r0        
Next_CMP         
        CMP     r14,#0x30000000
        BLS     End_CMP
        MOV      r2,#0x30000000
        MOV      r1,#0x40000000
        MOV      r0,#0
CP_Loop 
        LDR      r3,[r2],#4
        ADD      r0,r0,#1
        CMP      r0,#0x400
        STR      r3,[r1],#4
        BCC      CP_Loop                
        MOV      r0,#0x40000000
        MOV      pc,r0   
        
            
End_CMP 
        MOV     R0, #0xd1
        MSR     CPSR_c, R0
        LDR     R13, =FIQ_Stack     
        MOV     R0, #0xd2
        MSR     CPSR_c, R0
        LDR     R13, =IRQ_Stack 			           			
        MOV     R0, #0xd3
        MSR     CPSR_c, R0
        LDR     R13, =SVC_Stack 			
        MOV     R0, #0xd7
        MSR     CPSR_c, R0
        LDR     R13, =SVC_Stack 			
        MOV     R0, #0xdb
        MSR     CPSR_c, R0
        LDR     R13, =SVC_Stack 			
        MOV     R0, #0xdf
        MSR     CPSR_c, R0
        LDR     R13, =USR_Stack          

       ; IMPORT      Image_RO_Limit      	/* End of ROM code (=start of ROM data) */
       ; IMPORT      Image_RW_Base       	/* Base of RAM to initialise */
       ; IMPORT      Image_ZI_Base       	/* Base and limit of area */
       ; IMPORT      Image_ZI_Limit      	/* to zero initialise */
       ;
       ; ldr         r0, =Image_RO_Limit 	/* Get pointer to ROM data */
       ; ldr         r1, =Image_RW_Base  	/* and RAM copy */
       ; ldr         r3, =Image_ZI_Base  	/* Zero init base => top of initialised data */
		
		;====================
		IMPORT  |Image$$RO$$Limit|  ; End of ROM code (=start of ROM data)
		IMPORT  |Image$$RW$$Base|   ; Base of RAM to initialise
		IMPORT  |Image$$ZI$$Base|   ; Base and limit of area
		IMPORT  |Image$$ZI$$Limit|  ; to zero initialise
       
        ;Copy and paste RW data/zero initialized data
		ldr	r0, =|Image$$RO$$Limit| ; Get pointer to ROM data
		ldr	r1, =|Image$$RW$$Base|  ; and RAM copy
		ldr	r3, =|Image$$ZI$$Base|  ; Zero init base => top of initialised data 
		;================ 
        cmp         r0, r1                ; Check that they are different 
        beq         NoRW
LoopRw  CMP     	r1, r3  			; copy init data 
        LDRCC   	r2, [r0], #4
        STRCC   	r2, [r1], #4
        bcc     	LoopRw
NoRW    LDR     	r1, =|Image$$ZI$$Limit|  ; top of zero init segment 
        MOV     	r2, #0
LoopZI  CMP     	r3, r1  			; zero init 
        STRCC   	r2, [r3], #4
        beq     	LoopZI    
        			           
        MOV     R0, #0x1f
        MSR     CPSR_c, R0
        BL      Main     

;EXPORT  dmult
;IMPORT  dmult   
dmult 
		STMFD   sp!, {r4-r5} 	
		UMULL   r4,r5,r0,r1
        STR     r5,[r2,#0]
        STR     r4,[r3,#0]
        LDMFD   sp!, {r4-r5}      			
        MOV     pc,lr
  
  END